﻿using Emgu.CV.Structure;
using FinalYear.Helpers;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;

namespace FinalYear.iNavigate.Classes
{
    /// <summary>
    /// Calculating the optical flow magnitude of the
    /// points in the center of the scr
    /// </summary>
    public class EuclideanDistance : IDisposable
    {
        // private LineSegment2DF line { get; set; }

        public double hypotenuse { get; set; }

        public PointF originalPoint { get; set; }

        public PointF translatedPoint { get; set; }

        /// <summary>
        /// Initializes a new instance of the <see cref="EuclideanDistance" /> class.
        /// </summary>
        /// <param name="p1">The p1.</param>
        /// <param name="p2">The p2.</param>
        public EuclideanDistance(PointF p1, PointF p2)
        {
            this.originalPoint = p1;
            this.translatedPoint = p2;
            this.hypotenuse = Math.Sqrt(Math.Pow((double)originalPoint.X - translatedPoint.X, 2) + Math.Pow((double)originalPoint.Y - translatedPoint.Y, 2));
            //this.line = new LineSegment2DF(p1, p2);
            //this.hypotenuse = this.line.Length;
            Debug.WriteLine("The Flow Magnitude is: " + this.hypotenuse);
        }

        public EuclideanDistance()
        {
            
        }

        public void Dispose()
        {
            this.Dispose();
        }
    }

    /// <summary>
    /// List of euclidean distances
    /// </summary>
    public class EuclideanDistances : List<EuclideanDistance>
    {
        private int maximumHypotenuse;

        public int MaximumHypotenuse
        {
            get
            {
                if (this.maximumHypotenuse == 0)
                {
                    return Constants.MAXIMUMHYPOTENUSE;
                }

                return this.maximumHypotenuse;
            }
            set
            {
                this.maximumHypotenuse = value;
            }
        }

        public EuclideanDistances()
        {
        }

        public EuclideanDistances(int maxHypotenuse)
        {
            this.MaximumHypotenuse = maxHypotenuse;
        }

        /// <summary>
        /// Checks the center collision.
        /// </summary>
        /// <returns>
        /// True if the flow magnitude in the center region is 
        /// beyond a certain threshold
        /// </returns>
        public bool CheckCenterCollision()
        {
            int collisionCount = 0;
            if (this.Count() > 0)
            {
                for (int i = 0; i < this.Count(); i++)
                {
                    Debug.WriteLine("Count {0} | Collision Value {1}", i, this[i].hypotenuse);
                    if (this[i].hypotenuse > this.maximumHypotenuse)
                    {
                        collisionCount++;
                    }
                }
            }

            // if there is a collision count for more than half of the magnitudes within
            // the centre area.. then
            if (collisionCount > this.Count() / 4)
            {
                Debug.WriteLine("There is an impending collision with this set of central coordinates");
                return true;
            }

            return false;
        }

        /// <summary>
        /// Harvests the FOE.
        /// </summary>
        /// <param name="originalPoint">The original point.</param>
        /// <param name="matchedPoint">The matched point.</param>
        public void HarvestFOE()
        {
            LineEquations allPoints = new LineEquations();
            for (int i = 0; i < this.Count(); i++)
            {
                LineEquation currentEquation = new LineEquation();
                currentEquation.gradient = Convert.ToInt32(this[i].translatedPoint.Y - this[i].originalPoint.Y / this[i].translatedPoint.X - this[i].originalPoint.X);
                currentEquation.intersect = Convert.ToInt32(this[i].translatedPoint.Y - (currentEquation.gradient * this[i].translatedPoint.X));

                PointF origin = new PointF();
                origin.X = 0;
                origin.Y = (currentEquation.gradient * origin.X) + currentEquation.intersect;

                PointF end = new PointF();
                end.X = Constants.FRAMEWIDTH;
                end.Y = (currentEquation.gradient * end.X) + currentEquation.intersect;

                currentEquation.startPoint = origin;
                currentEquation.endPoint = end;

                allPoints.Add(currentEquation);
            }

            allPoints.FOE();
        }
    }
}